﻿/// 单应矩阵精度分析
/// 自己写的SVD计算单应矩阵算法

#include <iostream>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/Geometry>

// opencv
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/core/eigen.hpp>

using namespace std;
using namespace cv;

#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif

/// 自己计算 H 矩阵
static void calHWithSvd(vector<Eigen::Vector3d> &plane_xy, vector<Eigen::Vector3d> &uv)
{
    // 自己直接计算
    Eigen::Matrix<double, 8, 8> A;
    A <<
     plane_xy[0][0],plane_xy[0][1],1, 0,0,0, -uv[0][0]*plane_xy[0][0], -uv[0][0]*plane_xy[0][1],
     0,0,0, plane_xy[0][0],plane_xy[0][1],1, -uv[0][1]*plane_xy[0][0], -uv[0][1]*plane_xy[0][1],
     plane_xy[1][0],plane_xy[1][1],1, 0,0,0, -uv[1][0]*plane_xy[1][0], -uv[1][0]*plane_xy[1][1],
     0,0,0, plane_xy[1][0],plane_xy[1][1],1, -uv[1][1]*plane_xy[1][0], -uv[1][1]*plane_xy[1][1],
     plane_xy[2][0],plane_xy[2][1],1, 0,0,0, -uv[2][0]*plane_xy[2][0], -uv[2][0]*plane_xy[2][1],
     0,0,0, plane_xy[2][0],plane_xy[2][1],1, -uv[2][1]*plane_xy[2][0], -uv[2][1]*plane_xy[2][1],
    plane_xy[3][0],plane_xy[3][1],1, 0,0,0, -uv[3][0]*plane_xy[3][0], -uv[3][0]*plane_xy[3][1],
    0,0,0, plane_xy[3][0],plane_xy[3][1],1, -uv[3][1]*plane_xy[3][0], -uv[3][1]*plane_xy[3][1];

    Eigen::Matrix<double, 8, 1> b;
    b << uv[0][0], uv[0][1],
            uv[1][0], uv[1][1],
            uv[2][0], uv[2][1],
            uv[3][0], uv[3][1];
    cout << "A:\n" << A<< endl;


    cout << "b:\n" << b.transpose()<< endl;
    Eigen::Matrix<double, 8, 1> H3 = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
    cout << "H3:\n" << H3 << endl;
}

/// 获取建立相机模型
static void getCameraModel(vector<Eigen::Vector3d> &p_plane1_homo, vector<Eigen::Vector3d> &p_plane1_UV)
{
    // 模拟yiying CT 设备，f 估计越是 8138

    // 相机内参
    Eigen::Matrix3d K;
    K <<    5120, 0.0, 512,
            0, 5120, 512,
            0, 0, 1;
    cout << "K:\n" << K << endl;

    // 标尺空间W
    std::vector<Eigen::Vector3d> p_plane1 = {
        {0, 0, 0},
        {60, 0, 0},
        {60, 40, 0},
        {0, 40, 0},
        {50, 80, 0},
        {70, 70, 0}
    };

    for_each(p_plane1.begin(), p_plane1.end(), [&](Eigen::Vector3d &it){
        Eigen::Vector3d p = it; p[2] =1; p_plane1_homo.push_back(p);});

    // 相机C 和 平面的矩阵转换关系
    Eigen::Isometry3d T_WtoC = Eigen::Isometry3d::Identity(); // 标尺到相机坐标转换
    T_WtoC.pretranslate(Eigen::Vector3d(-30, -20, 500));
    double yaw = 0 * M_PI / 180;
    double pitch = 0 * M_PI / 180;
    double roll = 30 * M_PI / 180;
    Eigen::Matrix3d m;
    // 旋转顺序为 roll, pitch, yaw
    m = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
    T_WtoC.rotate(m);
    cout << "T_WtoC:\n" << T_WtoC.matrix() << endl;

    std::vector<Eigen::Vector3d> p_plane1_C;
    cout << "points plane1 in W:\n";
    for_each(p_plane1.begin(), p_plane1.end(), [&](Eigen::Vector3d &it) {
        cout << it.transpose() << endl; p_plane1_C.push_back(T_WtoC*it);});

    cout << "points plane1 in C:\n";
    for_each(p_plane1_C.begin(), p_plane1_C.end(), [&](Eigen::Vector3d &it) { cout << it.transpose() << endl;});

    // 项目平面UV
    cout << "points plane1 in UV:\n";
    //std::vector<Eigen::Vector3d> p_plane1_UV;
    for_each(p_plane1_C.begin(), p_plane1_C.end(), [&](Eigen::Vector3d &it) { p_plane1_UV.push_back(K * it/it[2]); });
    for_each(p_plane1_UV.begin(), p_plane1_UV.end(), [&](Eigen::Vector3d &it) { cout << it.transpose() << endl; });

    Eigen::Matrix<double, 3, 4> K1;
    K1 << K(0,0), K(0,1), K(0,2), 0,
            K(1,0), K(1,1), K(1,2), 0,
            K(2,0), K(2,1), K(2,2), 0;

    cout << "K1:\n" << K1 << endl;
    Eigen::Matrix<double, 3, 4> H1 = K1 * T_WtoC.matrix();
    cout << "H1:\n" << H1/500 << endl;
    H1 = H1/500;

    Eigen::Matrix3d H;
    H << H1(0,0), H1(0,1), H1(0,3),
            H1(1,0), H1(1,1), H1(1,3),
            H1(2,0), H1(2,1), H1(2,3);

    cout << "mapped: \n";
    for_each(p_plane1_homo.begin(), p_plane1_homo.end(), [&](Eigen::Vector3d &it){
        Eigen::Vector3d p = H * it; cout << it.transpose() << " => " << p.transpose()/p[2] << endl;
    });

}

/// 验证单应矩阵
static void verifyHomo(vector<Point2f> &pts_src, vector<Point2f> &pts_dst)
{
//    Mat H1 = getPerspectiveTransform(pts_src, pts_dst);
    Mat H1 = findHomography(pts_src, pts_dst);
    Eigen::Matrix3d eH1;
    cv2eigen(H1, eH1);

    cout << "H1:\n" << H1 << endl;

    for (uint i = 0; i < pts_src.size(); i++) {
        Point2f &pt = pts_src[i];
        Eigen::Vector3d newPt = eH1 * Eigen::Vector3d(pt.x, pt.y, 1);
        cout << newPt.transpose()/newPt[2] <<" vs " << pts_dst[i] << endl;
    }

    //for_each(pts_dst.begin(), pts_dst.end(), [&](Point2f it){  cout << it << endl;});
}

/// 建立相机模型
static void demoCameraModel()
{
    cout.precision(15);
    vector<Eigen::Vector3d> p_plane1_homo;
    vector<Eigen::Vector3d> p_plane1_UV;
    getCameraModel(p_plane1_homo, p_plane1_UV);

    vector<Point2f> pts_src, pts_dst;
    cout << "points plane1 in W:\n";
    for_each(p_plane1_homo.begin(), p_plane1_homo.end(), [&](Eigen::Vector3d &it) {
        cout << it.transpose() << endl; pts_src.push_back(Point2f(it[0], it[1]));});
    cout << "points plane1 in UV\n";
    for_each(p_plane1_UV.begin(), p_plane1_UV.end(), [&](Eigen::Vector3d &it) {
        cout << it.transpose() << endl; pts_dst.push_back(Point2f(it[0], it[1]));});

    cout << "---homo 1---" << endl;
    verifyHomo(pts_src, pts_dst);

    // 加一点扰动
    cout << "---homo 2---" << endl;
    pts_dst[0].x += 1;
    verifyHomo(pts_src, pts_dst);
}

static void demoMapPoints()
{
    vector<Point2f> pts_src = {
        {83.3333, 83.3333},
        {83.3333, 416.667},
        {416.667, 416.667},
        {416.667, 83.3333}
    };

    vector<Point2f> pts_dst = {
        {153.801, 217.638},
        {170.746, 393.078},
        {321.615, 393.563},
        {340.058, 215.632}
    };

    Mat H1 = findHomography(pts_src, pts_dst, RANSAC);
    Eigen::Matrix3d eH1;
    cv2eigen(H1*500, eH1);

    cout << "H1:\n" << H1 << endl;

    for (int i = 0; i < pts_src.size(); i++) {
        Point2f &pt = pts_src[i];
        Eigen::Vector3d newPt = eH1 * Eigen::Vector3d(pt.x, pt.y, 1) ;
        cout << newPt.transpose() <<" vs " << pts_dst[i] << endl;
    }

    for_each(pts_dst.begin(), pts_dst.end(), [&](Point2f it){  cout << it << endl;});
}

const char * usage =
" cmd <option>"
"   1 - demoCameraModel 单应矩阵转化 \n"
"   2 - demoMapPoints 相机算法模型\n"
;

static void help()
{
    printf("%s\n", usage);
}

int main(int argc, char *argv[])
{
    int option = 2;
    if (argc == 2) {
        option = atoi(argv[1]);
    }

    switch (option) {
    case 1:
        demoCameraModel();
        break;
    case 2:
        demoMapPoints();
        break;
    default:
        help();
        break;
    }
    return 0;
}
